
#include <openrave-core.h>
#include <vector>
#include <iostream>

#include "orexample.h"
#include "pidcontroller.hpp"
#include "jointspacesolver.hpp"
#include "trajectory.hpp"
#include "spline.hpp"
#include "robotStruct.hpp"

#include "computedtorque.hpp"

#define TIMESTEP 10 //milliseconds

#define LINKLEN1 0.0
#define LINKWIDTH1 0.05
#define LINKHEIGHT1 0.05
#define LINKCOM1 0.0
#define LINKMASS1 0.0


#define LINKLEN2 0.200        // THIS IS A CYLINDER RADIUS!!
#define LINKWIDTH2 0.05
#define LINKHEIGHT2 0.05
#define LINKCOM2 0.1
#define LINKMASS2 0.5

#define LINKLEN3 0.320
#define LINKWIDTH3 0.05
#define LINKHEIGHT3 0.05
#define LINKCOM3 0.1
#define LINKMASS3 0.5

#define LINKLEN4 0.550
#define LINKWIDTH4 0.05
#define LINKHEIGHT4 0.05
#define LINKCOM4 0.16
#define LINKMASS4 0.5

#define LINKLEN5 0.500
#define LINKWIDTH5 0.05
#define LINKHEIGHT5 0.05
#define LINKCOM5 0.14
#define LINKMASS5 0.5






using std::cout;
using std::endl;
using std::string;

namespace cppexamples {

class RobotArm : public OpenRAVEExample {
 public:
  virtual void demothread(int argc, char ** argv) {
    
std::cout <<"HELLO\n";



    // load the world:
    if( !penv->Load("models/world.xml") ){
      std::cout << "Failed to load xml\n";
      return;
    }
    // attach a physics engine:
    penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv, "ode"));
    penv->GetPhysicsEngine()->SetGravity(OpenRAVE::Vector(0,0,-9.8));
    
    // Find the first robot and get a pointer to it:
    std::vector<OpenRAVE::RobotBasePtr> orRobotVec;
    penv->GetRobots(orRobotVec);
    if(orRobotVec.size() < 1) {
      std::cout << "No robots found in xml\n";
      return;
    }
    
    OpenRAVE::RobotBasePtr orRobot = orRobotVec.at(0);
    uint32_t numJoints = orRobot->GetJoints().size();
    
    std::cout << "Init Robot Model:\n";
    RobotModel* robot = new RobotModel;
    robot->setLengths(LINKLEN1, LINKLEN2, LINKLEN3, LINKLEN4, LINKLEN5);
//    robot->setMasses(LINKMASS1, LINKMASS2, LINKMASS3, LINKMASS4, LINKMASS5);
//   robot->setWidths(LINKWIDTH1, LINKWIDTH2, LINKWIDTH3, LINKWIDTH4, LINKWIDTH5);
//    robot->setHeights(LINKHEIGHT1, LINKHEIGHT2, LINKHEIGHT3, LINKHEIGHT4, LINKHEIGHT5);
//    robot->setCOM(LINKCOM1, LINKCOM2, LINKCOM3, LINKCOM4, LINKCOM5);        


 //   robot->setLengths(LINKLEN1, LINKLEN2, LINKLEN3, LINKLEN4, LINKLEN5);
    robot->setMasses(0, LINKMASS1, LINKMASS2, LINKMASS3, LINKMASS4);
    robot->setWidths(0, LINKWIDTH1, LINKWIDTH2, LINKWIDTH3, LINKWIDTH4);
    robot->setHeights(0, LINKHEIGHT1, LINKHEIGHT2, LINKHEIGHT3, LINKHEIGHT4);
    robot->setCOM(0, LINKCOM1, LINKCOM2, LINKCOM3, LINKCOM4);   



    cout << "Forming Joint Trajectory...\n";
    assert(numJoints == 4);
    
    // ---------------- PLAN PATH ---------------- //
    
    JointSpaceSolver* solver = new JointSpaceSolver(robot);
    
    double reach = solver->getReachableLimit()-0.0001;
    
    Trajectory* trajectory = new Trajectory();
    Setpoint* sp = new Setpoint(0, true);
    
    sp->time = 0;
    sp->cpos->set(reach, 0.0, 0.0, M_PI/3);
    if(solver->solve(sp) != 0) return;
    trajectory->addSetpoint(sp);
    
    
    // ---- WOBBLY CIRCLE ----
    double path_r = 2*reach/3;
    double path_a = reach/12;
    
    sp->time = 1000;
    sp->cpos->set(path_r*cos(0), path_r*sin(0), 0, M_PI/3);
    if(solver->solve(sp) != 0) return;
    trajectory->addSetpoint(sp);
    
    for(double angle = 0; angle < 2*M_PI; angle += M_PI/180){
      sp->time = sp->time+100;
      double r = path_r + path_a*sin(angle*6);
      sp->cpos->set(r*cos(angle), r*sin(angle), 0, M_PI/3);
      if(solver->solve(sp) != 0) return;
      trajectory->addSetpoint(sp);
    }
    /*
    // ---- STRAIGHT LINE ----
    double path_x = 3*reach/4;
    double path_y = reach/2;
    
    sp->time = 3000;
    sp->cpos->set(path_x, path_y, 0, M_PI/3);
    if(solver->solve(sp) != 0) return;
    trajectory->addSetpoint(sp);
    
    for(path_x = 3*reach/4; path_x > -3*reach/4; path_x -= 0.05){
      sp->time = sp->time+500;
      sp->cpos->set(path_x, path_y, 0, M_PI/3);
      if(solver->solve(sp) != 0) return;
      trajectory->addSetpoint(sp);
    }
    */

    // Whole manipulator computed torque controller
    cout << "Constructing Computed Torque Controller...\n";
    ComputedTorqueCont* cont = new ComputedTorqueCont(robot);

    // Set stiffness
    double stiffness = 1000.0;

    cont->setKp(stiffness);
    cont->setKv(2*sqrt(stiffness));     // Critical damping

/*
    // Construct Controllers:
    cout << "Constructing Controllers...\n";
    
    std::vector<PIDController*> controllers;
    controllers.resize(numJoints);
    
    // PID Gains:
    double Kp = 200.0;
    double Ki = 0.7;
    double Kd = 1000;
    
    // Max Joint Strength:
    double maxTorque = 100; // Nm
    
    // Joint 0:
    controllers[0] = new PIDController(Kp, Ki, Kd);
    controllers[0]->setAntiWindup(-maxTorque, maxTorque);

    // Joint 1:
    controllers[1] = new PIDController(Kp, Ki, Kd);
    controllers[1]->setAntiWindup(-maxTorque, maxTorque);
    
    // Joint 2:
    controllers[2] = new PIDController(Kp, Ki, Kd);
    controllers[2]->setAntiWindup(-maxTorque, maxTorque);
    
    // Joint 3:
    controllers[3] = new PIDController(Kp, Ki, Kd);
    controllers[3]->setAntiWindup(-maxTorque, maxTorque);
*/



    cout << "Entering loop...\n";
    uint64_t simtime = 0.0;
    
    std::vector<OpenRAVE::dReal> positions;        // angular position
    positions.resize(numJoints);

    std::vector<OpenRAVE::dReal> velocities;       // angular velocities
    velocities.resize(numJoints);   

/*    
    std::vector<OpenRAVE::dReal> accelerations;       // angular accelerations
    accelerations.resize(numJoints);  
    for(unsigned int i=0; i<accelerations.size(); i++) {
      accelerations[i] = 0.0;
    }
*/

    std::vector<OpenRAVE::dReal> q_d;
    q_d.resize(numJoints);
    std::vector<OpenRAVE::dReal> q_dot_d;
    q_dot_d.resize(numJoints);
    std::vector<OpenRAVE::dReal> q_ddot_d;
    q_ddot_d.resize(numJoints);

    std::vector<OpenRAVE::dReal> torques;          // joint torques
    torques.resize(numJoints);
    
    while(IsOk()) {
      {  // Begin Mutually Exclusive Segment:
        OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
        
        simtime = penv->GetSimulationTime()/1000;    // milliseconds
        //cout << "time = " << simtime << "ms \n";
        // Get Joint positions:
        orRobot->GetDOFValues(positions);
        orRobot->GetDOFVelocities(velocities);


        // Get Joint set points:
        trajectory->getSetpoint(simtime, sp);   // to repeat use (simtime%12000)

        for(uint32_t i=0; i<numJoints; i++) {
          q_d[i] = sp->jpos->get(i);
          q_dot_d[i] = sp->jvel->get(i);
          q_ddot_d[i] = sp->jacc->get(i);
        }


        cont->compute(positions, velocities, q_d, q_dot_d, q_ddot_d, torques);
        



        // for(uint32_t i=0; i<numJoints; i++) {
        //   /*
        //   cout << "Joint:" << i << "  Pos:" << positions.at(i) 
        //        << "  Set Point:" << sp->at(i) << "\n";
        //   */
        //   // Get control signal from PID:
        //   double controlSignal = controllers.at(i)->compute(positions.at(i), 
        //                                                  sp->jpos->get(i));
          
        //   // Max/Min to Actuator rails:
        //   double torque = controlSignal;
        //   if( controlSignal >  maxTorque ) torque =  maxTorque;
        //   if( controlSignal < -maxTorque ) torque = -maxTorque;
          
        //   torques.at(i) = torque;
        //   //cout << "  Torque:" << torques.at(i) << "\n";
        // }

        std::cout << "To Controllers: ";
        for(unsigned int i=0; i<torques.size(); i++) {
            std::cout << torques[i] << ", ";
        }
        std::cout << std::endl;
        // Set joint torques
        orRobot->SetDOFTorques(torques, false);
      }  // end Mutually Exclusive Segment
      
      std::list<OpenRAVE::GraphHandlePtr> listgraphs;
      listgraphs.clear();
      std::vector< OpenRAVE::RaveVector<float> > vpoints;
      
      double dscale = 1.85;
      
      for(long ti = 0; ti < 50000; ti += 500) {
        vpoints.clear();
        
        trajectory->getSetpoint(ti, sp);
        OpenRAVE::RaveVector<float> rv1(sp->cpos->x*dscale, sp->cpos->y*dscale, sp->cpos->z*dscale+1.8);
        vpoints.push_back(rv1);
        
        trajectory->getSetpoint(ti+500, sp);
        //sp->println();
        OpenRAVE::RaveVector<float> rv2(sp->cpos->x*dscale, sp->cpos->y*dscale, sp->cpos->z*dscale+1.8);
        vpoints.push_back(rv2);
        
        listgraphs.push_back(penv->drawlinestrip(&vpoints[0].x, vpoints.size(), sizeof(vpoints[0]), 1.0f));
      }
      
      while( penv->GetSimulationTime()/1000 - simtime < TIMESTEP && IsOk()) {
        // wait...
      }
    }
    // Cleanup:
    delete sp;
    delete robot;
    delete solver;
    delete trajectory;
//    for(uint32_t i = 0; i < controllers.size(); i ++) {
//      delete controllers.at(i);
//    }
  }
};  // end class

} // end namespace cppexamples
